#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
std::string turtlename;
/*
        <node pkg = "turtle_follow" type = "turtle_fl_client" name = "turtle2"  output = "screen" />

        <node pkg = "turtle_follow" type = "turtle_fl_pub" name = "pub1"  output = "screen" args= "turtle1" />
        <node pkg = "turtle_follow" type = "turtle_fl_pub" name = "pub2"  output = "screen" args= "turtle2" />

        <node pkg = "turtle_follow" type = "turtle_fl_sub" name = "sub"  output = "screen"  />
*/
void docallback(const turtlesim::Pose::ConstPtr& pose)
{
        static tf2_ros::TransformBroadcaster tfb;
        geometry_msgs::TransformStamped tfs;
        tfs.header.stamp = ros::Time::now();
        tfs.header.frame_id = "world";
        tfs.child_frame_id = turtlename;

        tfs.transform.translation.x = pose->x;
        tfs.transform.translation.y = pose->y;
        tfs.transform.translation.z = 0.0;

        tf2::Quaternion qtn;
        qtn.setRPY(0,0,pose->theta);
        tfs.transform.rotation.x = qtn.getX();
        tfs.transform.rotation.y = qtn.getY();
        tfs.transform.rotation.z = qtn.getZ();
        tfs.transform.rotation.w = qtn.getW();

        tfb.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
        setlocale(LC_ALL,"");

        ros::init(argc,argv,"turtle_pub");
        ros::NodeHandle nh;

        if (argc!=2)
        {                
                ROS_INFO("please input correct parameter");
        }
        else
        {
              turtlename  = argv[1];
              ROS_INFO(" %s is running",turtlename.c_str());
        }
        //create subscriber
        ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtlename+"/pose",1000,docallback);

        ros::spin();
        return 0;
}